
#define ONBOARD_PARAM_NAME_LENGTH 15

static uint8_t mav_var_type(AP_Meta_class::Type_id t)
{
	return 0;
}

// true when we have received at least 1 MAVLink packet
static bool mavlink_active;

GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) :
packet_drops(0),

// parameters
// note, all values not explicitly initialised here are zeroed
waypoint_send_timeout(1000), // 1 second
waypoint_receive_timeout(1000), // 1 second

// stream rates
_group	(key, key == Parameters::k_param_streamrates_port0 ? PSTR("SR0_"): PSTR("SR3_")),
				// AP_VAR					//ref	 //index, default, 	name
				streamRateRawSensors		(&_group, 0, 		0,		 PSTR("RAW_SENS")),
				streamRateExtendedStatus	(&_group, 1, 		0,		 PSTR("EXT_STAT")),
				streamRateRCChannels		(&_group, 2, 		0,		 PSTR("RC_CHAN")),
				streamRateRawController		(&_group, 3, 		0,		 PSTR("RAW_CTRL")),
				streamRatePosition			(&_group, 4, 		0,		 PSTR("POSITION")),
				streamRateExtra1			(&_group, 5, 		0,		 PSTR("EXTRA1")),
				streamRateExtra2			(&_group, 6, 		0,		 PSTR("EXTRA2")),
				streamRateExtra3			(&_group, 7, 		0,		 PSTR("EXTRA3"))
{

}

void
GCS_MAVLINK::init(FastSerial * port)
{
    GCS_Class::init(port);
    
        mavlink_comm_0_port = port;
        chan = MAVLINK_COMM_0;
        
	_queued_parameter = NULL;
}


void
GCS_MAVLINK::update(void)
{
    // receive new packets
    mavlink_message_t msg;
    mavlink_status_t status;
	status.packet_rx_drop_count = 0;
    // process received bytes
    while (comm_get_available(chan))
    {
        uint8_t c = comm_receive_ch(chan);
        
        Serial.write(c);

#if CLI_ENABLED == ENABLED
        /* allow CLI to be started by hitting enter 3 times, if no
           heartbeat packets have been received */
        if (mavlink_active == 0 && millis() < 20000) {
            if (c == '\n' || c == '\r') {
                crlf_count++;
            } else {
                crlf_count = 0;
            }
            if (crlf_count == 3) {
//                run_cli();
            }
        }
#endif

        // Try to get a new message
        if (mavlink_parse_char(chan, c, &msg, &status)) {
            mavlink_active = 1;
            handleMessage(&msg);
        }
    }

    // Update packet drops counter
    packet_drops += status.packet_rx_drop_count;

    // send out queued params/ waypoints
    if (NULL != _queued_parameter) {
//        send_message(MSG_NEXT_PARAM);
    }

    if (!waypoint_receiving && !waypoint_sending) {
        return;
    }

    uint32_t tnow = millis();
/*
    if (waypoint_receiving &&
        waypoint_request_i <= (unsigned)g.command_total &&
        tnow > waypoint_timelast_request + 500) {
        waypoint_timelast_request = tnow;
        send_message(MSG_NEXT_WAYPOINT);
    }

    // stop waypoint sending if timeout
    if (waypoint_sending && (millis() - waypoint_timelast_send) > waypoint_send_timeout){
        waypoint_sending = false;
    }

    // stop waypoint receiving if timeout
    if (waypoint_receiving && (millis() - waypoint_timelast_receive) > waypoint_receive_timeout){
        waypoint_receiving = false;
    }
    */
}


void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
{
    switch (msg->msgid) {

    case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
        {
             // decode
            mavlink_nav_controller_output_t packet;
            mavlink_msg_nav_controller_output_decode(msg, &packet);
            
          break;
        }
            case MAVLINK_MSG_ID_SYS_STATUS:
        {
                       // decode
            mavlink_sys_status_t packet;
            mavlink_msg_sys_status_decode(msg, &packet);
            
          break;
        }
            case MAVLINK_MSG_ID_ATTITUDE:
        {
                       // decode
            mavlink_attitude_t packet;
            mavlink_msg_attitude_decode(msg, &packet);
            
            roll = ToDeg(packet.roll);
                        pitch = ToDeg(packet.pitch);
                                    yaw = ToDeg(packet.yaw);
          break;
        }
            case MAVLINK_MSG_ID_GPS_RAW:
        {
                       // decode
            mavlink_gps_raw_t packet;
            mavlink_msg_gps_raw_decode(msg, &packet);
            
          break;
        }
            case MAVLINK_MSG_ID_GPS_STATUS:
        {
                       // decode
            mavlink_gps_status_t packet;
            mavlink_msg_gps_status_decode(msg, &packet);
            
          break;
        }
            case MAVLINK_MSG_ID_WAYPOINT_CURRENT:
        {
                       // decode
            mavlink_waypoint_current_t packet;
            mavlink_msg_waypoint_current_decode(msg, &packet);
            
          break;
        }
            case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
        {             
          // decode
            mavlink_rc_channels_raw_t packet;
            mavlink_msg_rc_channels_raw_decode(msg, &packet);
          
          break;
        }
            case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
        {
                       // decode
            mavlink_servo_output_raw_t packet;
            mavlink_msg_servo_output_raw_decode(msg, &packet);
            
          break;
        }
            case MAVLINK_MSG_ID_RAW_IMU:
        {
                       // decode
            mavlink_raw_imu_t packet;
            mavlink_msg_raw_imu_decode(msg, &packet);
            
          break;
        }
            case MAVLINK_MSG_ID_SCALED_IMU:
        {
                       // decode
            mavlink_scaled_imu_t packet;
            mavlink_msg_scaled_imu_decode(msg, &packet);
            
          break;
        }
            case MAVLINK_MSG_ID_VFR_HUD:
        {
                       // decode
            mavlink_vfr_hud_t packet;
            mavlink_msg_vfr_hud_decode(msg, &packet);
            
          break;
        }
    } // end switch
} // end handle mavlink

uint16_t
GCS_MAVLINK::_count_parameters()
{
    // if we haven't cached the parameter count yet...
    if (0 == _parameter_count) {
        AP_Var  *vp;

        vp = AP_Var::first();
        do {
            // if a parameter responds to cast_to_float then we are going to be able to report it
            if (!isnan(vp->cast_to_float())) {
                _parameter_count++;
            }
        } while (NULL != (vp = vp->next()));
    }
    return _parameter_count;
}

AP_Var *
GCS_MAVLINK::_find_parameter(uint16_t index)
{
    AP_Var  *vp;

    vp = AP_Var::first();
    while (NULL != vp) {

        // if the parameter is reportable
        if (!(isnan(vp->cast_to_float()))) {
            // if we have counted down to the index we want
            if (0 == index) {
                // return the parameter
                return vp;
            }
            // count off this parameter, as it is reportable but not
            // the one we want
            index--;
        }
        // and move to the next parameter
        vp = vp->next();
    }
    return NULL;
}

/**
* @brief Send the next pending parameter, called from deferred message
* handling code
*/
void
GCS_MAVLINK::queued_param_send()
{
    // Check to see if we are sending parameters
    if (NULL == _queued_parameter) return;

    AP_Var      *vp;
    float       value;

    // copy the current parameter and prepare to move to the next
    vp = _queued_parameter;
    _queued_parameter = _queued_parameter->next();

    // if the parameter can be cast to float, report it here and break out of the loop
    value = vp->cast_to_float();
    if (!isnan(value)) {
        char param_name[ONBOARD_PARAM_NAME_LENGTH];         /// XXX HACK
        vp->copy_name(param_name, sizeof(param_name));

        mavlink_msg_param_value_send(
            chan,
            (int8_t *)param_name,
            value,
//            mav_var_type(vp->meta_type_id()),
            _queued_parameter_count,
            _queued_parameter_index);

        _queued_parameter_index++;
    }
}

